﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Mechatronics.Robotics.Protocols
{
    public class SilverProtocol
    {
        public byte[] SetSpeedRequest(double left, double right)
        {
            if (left < -1 || left > 1)
                throw new ArgumentOutOfRangeException("left");

            if (right < -1 || right > 1)
                throw new ArgumentOutOfRangeException("right");

            // converting values from -1.0 and 1.0 to 0 - 510
            int l = Convert.ToInt32((left + 1.0) * 510.0 / 2.0);
            int r = Convert.ToInt32((right + 1.0) * 510.0 / 2.0);

            if (l < 0)
                l = 0;
            if (l > 510)
                l = 510;
            if (r < 0)
                r = 0;
            if (r > 510)
                r = 510;

            return Encoding.ASCII.GetBytes(string.Format(":SP {0},{1}\r\n", l, r));
        }

        public byte[] ReadBatteryRequest()
        {
            return Encoding.ASCII.GetBytes(":BATT?");
        }

        public double ParseReadBatteryResponse(byte[] data)
        {
            if (data == null)
                throw new ArgumentNullException();

            string response = Encoding.ASCII.GetString(data);

            if (string.IsNullOrEmpty(response))
                throw new ArgumentException("Bad response from robot, propably communication error");

            // basic check
            int batt;
            if (!int.TryParse(response, out batt))
                throw new ArgumentException("Bad response from robot, propably communication error"); ;

            if (batt < 0 || batt > 240)
                throw new Exception("Battery response out of range, propably communication error");

            return (double)batt * 0.1;
        }

        public byte[] ReadSonarRequest()
        {
            return Encoding.ASCII.GetBytes(":PING?\r\n");
        }

        public void ParseReadSonarResponse(byte[] data, out int ping, out double sharp)
        {
            if (data == null)
                throw new ArgumentNullException();

            string response = Encoding.ASCII.GetString(data);

            if (string.IsNullOrEmpty(response))
                throw new ArgumentException("Bad response from robot, propably communication error");

            string[] fields = response.Split(',');
            if (fields.Length != 2)
                throw new ArgumentException("Bad response from robot, propably communication error");

            if (!int.TryParse(fields[0], out ping))
                throw new ArgumentException("Bad response from robot, propably communication error");

            int isharp;
            if (!int.TryParse(fields[1], out isharp))
                throw new ArgumentException("Bad response from robot, propably communication error");

            // if we reached here, data seems to be ok
            sharp = (double)isharp * 0.0784;
        }

        public int[] ParseReadIRResponse(byte[] data)
        {
            if (data == null)
                throw new ArgumentNullException();

            string response = Encoding.ASCII.GetString(data);

            if (string.IsNullOrEmpty(response))
                throw new ArgumentException("Bad response from robot, propably communication error");

            string[] fields = response.Split(',');
            if (fields.Length != 10)
                throw new ArgumentException("Bad response from robot, propably communication error");

            int[] ret = new int[8];
            for (int i = 0; i < 8; ++i)
            {
                if (!int.TryParse(fields[i], out ret[i]))
                    throw new ArgumentException("Bad response from robot, propably communication error");
            }

            return ret;
        }

        public byte[] ReadIRRequest()
        {
            return Encoding.ASCII.GetBytes(":MEAS?");
        }

        public byte[] ParseResponse(byte[] response)
        {
            // basic checking
            if (response.Length < 3 ||
                response[response.Length - 1] != '\n' ||
                response[response.Length - 2] != '\r')
                throw new Exception("ParseResponse() Bad formatted response from robot. Propably communication problem");

            // reading answer
            byte[] ret = new byte[response.Length - 2];
            Array.Copy(response, ret, response.Length - 2);
            return ret;
        }

        //internal void ParseReadIRResponse()
        //{

        //    string response = Connection.ReadResponse();
        //    if (response == null)
        //        return;

        //    string[] fields = response.Split(',');

        //    // if there's too many fields in answer
        //    if (fields.Length != 10)
        //        return;

        //    // parsing Sharps data
        //    int[] new_sharps = new int[8];
        //    for (int i = 0; i < 8; ++i)
        //    {
        //        int val;
        //        if (!int.TryParse(fields[i], out val))
        //            return;
        //        new_sharps[i] = val;
        //    }

        //    // parsing Encoders data
        //    int left_encoder;
        //    if (!int.TryParse(fields[8], out left_encoder))
        //        return;

        //    int right_encoder;
        //    if (!int.TryParse(fields[9], out right_encoder))
        //        return;

        //    // if we reached here, data seems to be ok
        //    _state.EncoderLeft = left_encoder;
        //    _state.EncoderRight = right_encoder;
        //    for (int i = 0; i < 8; ++i)
        //        _state.Sharps[i] = (double)new_sharps[i] * SharpUnit;

        //}
    }
}
